#pragma once

#include "vio/front_end/camera.h"
#include "vio/front_end/feature.h"
#include "vio/vio_common.h"

namespace vio {
struct Feature;

typedef std::list<std::shared_ptr<Feature>> Features;
typedef std::vector<cv::Mat> ImgPyr;

// a frame saves the image, the associated features and the estimated pose.
class Frame : private boost::noncopyable {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  typedef std::shared_ptr<Frame> Ptr;

 public:
  Frame(const Camera::Ptr cam, const cv::Mat& img, const double& ts);

  // initialize new frame and create image pyramid
  void InitFrame(const cv::Mat& img);

  // select this frame as keyframe
  void SetKeyFrame();

  // add a feature to the image
  void AddFeature(std::shared_ptr<Feature> feat_ptr);

  // the keypionts are those five features which are closest to the 4 image
  // corners and to the center and which have a 3D point assigned. these points
  // are used to quickly check whether two frames have overlapping filed of
  // view.
  void SetKeyPoints();

  // check if we can select five better keypoints.
  void CheckKeyPoints(std::shared_ptr<Feature> feat_ptr);

  // if a point is deleted, we must remove the corresponding keypoint
  void RemoveKeyPoint(std::shared_ptr<Feature>& feat_ptr);

  // return number of point observations
  inline size_t FeatureObs() const {
    return features_.size();
  }

  // check if a point in world frame is visible in the image
  bool IsVisible(const Eigen::Vector3d& p3d_w) const;

  // full resolution image stored in the frame
  inline const cv::Mat& Img() const {
    return img_pyr_[0];
  }

  // was this frame selected as keyframe
  inline bool IsKeyFrame() const {
    return is_keyframe_;
  }

  // Transforms world frame(w) to camera pixel frame(p)
  inline Eigen::Vector2d w2p(const Eigen::Vector3d& p3_w) const {
    return cam_ptr_->world2pixel(p3_w, T_f_w_);
  }

  // pixel frame(p) to unit sphere camera frame(u)
  inline Eigen::Vector3d p2u(const Eigen::Vector2d& p2_p) const {
    return cam_ptr_->pixel2camera(p2_p);
  }

  // pixel frame(p) to unit sphere camera frame(u)
  inline Eigen::Vector3d p2u(const double& px, const double& py) const {
    return cam_ptr_->pixel2camera(Eigen::Vector2d(px, py));
  }

  // world frame(w) to camera frame(c)
  inline Eigen::Vector3d w2c(const Eigen::Vector3d& p3_w) const {
    return T_f_w_ * p3_w;
  }

  // camera frame(c) to world frame(w)
  inline Eigen::Vector3d c2w(const Eigen::Vector3d& p3_c) const {
    return T_f_w_.inverse() * p3_c;
  }

  // projects point from unit sphere(u) in camera pixel frame(p)
  inline Eigen::Vector2d u2p(const Eigen::Vector3d& p3_u) const {
    return cam_ptr_->camera2pixel(p3_u);
  }

  // return the pose of the frame in the world frame
  inline Eigen::Vector3d pos() const {
    return T_f_w_.inverse().translation();
  }

  // frame jacobian for projection of 3D point in camera frame to unit plane
  // frame uv(focal length = 1) (7.46)
  inline static void jacobian_xyz2uv(const Eigen::Vector3d& xyz_cam,
                                     Eigen::Matrix<double, 2, 6>& J) {
    const double x = xyz_cam[0];  // fx * x
    const double y = xyz_cam[1];  // fy * y
    const double z_inv = 1.0 / xyz_cam[2];
    const double z_inv_2 = z_inv * z_inv;

    J(0, 0) = -z_inv;
    J(0, 1) = 0.0;
    J(0, 2) = x * z_inv_2;
    J(0, 3) = y * J(0, 2);
    J(0, 4) = -(1.0 + x * J(0, 2));
    J(0, 5) = y * z_inv;

    J(1, 0) = 0.0;
    J(1, 1) = -z_inv;
    J(1, 2) = y * z_inv_2;
    J(1, 3) = 1.0 + y * J(1, 2);
    J(1, 4) = -J(0, 3);
    J(1, 5) = -x * z_inv;
  }

 public:
  static int frame_counter_;         // counts the number of created
                                     // frames, used to set the unique id.
  int id_;                           // unique id of frame
  double timestamp_;                 // timestamp of frame
  Camera::Ptr cam_ptr_;              // camera model
  Sophus::SE3d T_f_w_;               // world frame(w) to camera frame(f)
  Eigen::Matrix<double, 6, 6> cov_;  // convariance of frame pose
  ImgPyr img_pyr_;                   // image pyramid
  Features features_;                // list of features in the image
  std::vector<std::shared_ptr<Feature>>
      key_pts_;  // five features and associated 3D points which are used to
                 // detect if two frames have overlapping file of view.
  bool is_keyframe_;       // was this frames selected as keyframe?
  int last_published_ts_;  // timestamp(us) of last publishing
};

namespace frame_utils {
// create an image pyramid of half-sampled images
void CreateImgPyramid(const cv::Mat& img_level_0,
                      const int& n_levels,
                      ImgPyr& pyr);

// get the average depth of the features in the image
bool GetScenseDepth(const Frame& frame, double& depth_mean, double& depth_min);
}  // namespace frame_utils
}  // namespace vio
